import time

import open3d as o3d
import yaml

from cuboid_locate import cuboid_locate


def main():
    cfg_file_path = "config.yaml"
    with open(cfg_file_path, encoding="utf-8") as cfg_file:
        config_data = yaml.load(cfg_file, Loader=yaml.FullLoader)

    pcd_file_path = config_data["pcd_file_path"]
    cuboid_size = config_data["cuboid_size"]
    rough_location = config_data["rough_location"]
    spot_distance = config_data["spot_distance"]

    x_range = rough_location["x_range"]
    y_range = rough_location["y_range"]
    z_range = rough_location["z_range"]
    cuboid_length = cuboid_size["length"]
    cuboid_width = cuboid_size["width"]
    origin_pcd = o3d.io.read_point_cloud(pcd_file_path)

    count = 100
    start_time = time.time()
    for _ in range(count):
        pcd, surface = cuboid_locate(origin_pcd, x_range, y_range, z_range,
                                     spot_distance, cuboid_length, cuboid_width)
    end_time = time.time()
    print(F"average process time: {(end_time - start_time) / count:.5f}")

    print(F"top left: {surface.top_left.x}, {surface.top_left.y}")
    print(F"top right: {surface.top_right.x}, {surface.top_right.y}")
    print(F"bottom left: {surface.bottom_left.x}, {surface.bottom_left.y}")
    print(F"bottom right: {surface.bottom_right.x}, {surface.bottom_right.y}")
    o3d.visualization.draw_geometries([pcd])


if __name__ == "__main__":
    main()
